/* Para resolver sistemas de ecuaciones, invertir matrices, 
 * descomponer matrices, y multiplicar matrices por vectores. */
#include "sistemas.h"

/* a/b */
double dividirEnRango(double a, double b)
{
	if (fabs(a) <= 1e-16 || fabs(b) <= 1e-16)		
		return 1e-16;
	else
		return a / b;
}

void invertirTriangInf(Matriz& A, Matriz& res, int n) /* A triangular inferior (Lower). */
{
	double suma = 0;
	
	forn(i, n)
	{
		fornd(j, i+1)
		{
			if (i == j)
			{				
				res[i][j] = dividirEnRango(1.0, A[i][i]);
			}
			else
			{
				suma = 0;
				forn(k, i+1)
				{
					suma += A[i][k] * res[k][j];
				}
				
				res[i][j] = dividirEnRango(-suma, A[i][i]);
			}
		}
		
		fornx(j, i+1, n)
		{
			res[i][j] = 0;
		}
	}
	
	return;
}

void matrizPorVector(Matriz& A, Vector& x, Vector& res, int n) /* res = A.x */
{
	forn(i, n)
	{
		double aux = 0;
		
		forn(j, n)
		{
			aux += A[i][j] * x[j];
		}
		
		res[i] = aux;
	}
	
	return;	
}

void matrizPorDiagonal(Matriz &A, Vector &d, Matriz& res, int n) 
/* res = A.B donde B[i][j] = o si i != j, d[i] si i == j. */
{
	forn(i, n)
	{
		forn(j, n)
		{
			res[i][j] = d[j] * A[i][j];			
		}	
	}
	
	return;	
}

void diagonalPorMatriz(Vector &d, Matriz& A, Matriz& res, int n) 
/* res = B.A donde B[i][j] = o si i != j, d[i] si i == j. */
{
	forn(i, n)
	{
		forn(j, n)
		{		
			res[i][j] = d[i] * A[i][j];			
		}	
	}
	
	return;	
}

void backwardSubs(Matriz& L, Vector& b, Vector& x, int n) 
/* Resuelve el sistema triangular inferior Lx = b. */
{
	//~ x_i = b_{i} - \sum_{j=0}^{i-1} L[j][i]*x_j
	
	forn(i, n)
		x[i] = b[i];
	
	forn(i, n)
	{
		forn(j, i)
		{
			x[i] -= L[i][j] * x[j];
			//~ cout << "x[" << i << "] : " << x[i] << endl;
		}
		
		x[i] = dividirEnRango(x[i], L[i][i]);		
	}
	
	return;
}

void forwardSubs(Matriz& U, Vector& b, Vector& x, int n) 
/* Resuelve el sistema triangular superior Ux = b. */
{
	//~ x_i = (b_{i} - \sum_{j=i+1}^{n-1} U[i][j]*x_j) / u[i][i]
	
	forn(i, n)
		x[i] = b[i];
	
	fornd(i, n)
	{
		if (i < n - 1)
		{
			fornx(j, i+1, n)
			{
				x[i] -= U[i][j] * x[j];
				//~ cout << "x[" << i << "] : " << x[i] << endl;
			}
		}
		
		x[i] = dividirEnRango(x[i], U[i][i]);
	}
	
	return;
}

void intercambiarFilas(Matriz& A, Vector& v, Matriz& res, int n)
{
	forn(i, n)	
		res[i] = A[v[i]];	
}

int posmaximo(const Vector v, int n) 
{
    int pos_maximo = 0;
    for(int i = 1; i < n; i++) 
    {
        if(fabs(v[i]) > fabs(v[pos_maximo]))
            pos_maximo = i;
    }
    return pos_maximo;
}

void LU(const Matriz& M, Matriz& res, int n)
{
	/* Hacer que res sea inicialmente igual a M. */
	forn(i, n)
		res[i] = M[i];
	
	double beta;	
	
	/* Por cada columna. */
	forn(j, n-1)
	{		
		double coef_gauss = res[j][j];
		
		/* Para cada fila debajo de la diagonal j. */		
		fornx(i, j + 1, n)
		{
			beta = dividirEnRango(res[i][j], coef_gauss);
			/* El coeficiente se guarda en L. */
			res[i][j] = beta;
			
			/* Se modifica la matríz para ir obteniendo U. */
			fornx(k, j + 1, n)
			{
				res[i][k] -= beta*res[j][k];
			}
		}				
	}	
	
	return;
}

void PLUPivoteo(const Matriz& M, Vector& b, Vector& x, int n)
{
	Vector maximos(n, 0);	
	Matriz L (n, Vector(n,0));	
	Matriz U (n, Vector(n,0));	
		
	Matriz res (n, Vector(n));	

	/* Se busca el maximo de cada fila y se deja en el vector maximos*/
	forn(i, n)
	{		
		forn(j, n)
		{
			if(fabs(M[i][j]) >= fabs(maximos[i]))
			{
				maximos[i] = fabs(M[i][j]);
			}	
		}
	}
	
	/* Hacer que res sea inicialmente igual a M. */
	forn(i, n)
		res[i] = M[i];
	
	/* Por cada columna. */
	forn(j, n-1)
	{
		int p = 0;
		double max = 0;
		
		/* Dividir cada elemento de la fila por el máximo de la fila, 
		 * y quedarse con el máximo de estos valores. */
		fornx(i, j, n)
		{
			double aux = dividirEnRango(fabs(M[i][j]), maximos[i]);
						
			if (aux > max)
			{
				max = aux;
				p = i;
			}			
		}		
		
		/* Intercambiar fila j con fila p. */
		Vector aux = res[j];
		res[j] = res[p];
		res[p] = aux;
		
		/* Intercambia fila j con fila p también en b. */
		swap(b, j, p);
		
		/* Ahora res[j][j] es el pivote. */
		double pivote = res[j][j];
		
		/* Para cada fila debajo de la diagonal j. */		
		fornx(i, j + 1, n)
		{
			double beta = dividirEnRango(res[i][j], pivote);
			/* El coeficiente se guarda en L. */
			res[i][j] = beta;
			
			/* Se modifica la matríz para ir obteniendo U. */
			fornx(k, j + 1, n)
			{
				res[i][k] -= beta*res[j][k];
			}			
		}		
	}
	
	/* LU x = b 
	 * Ly = b backwardSubs
	 * Ux = y forwardSubs */
	
	forn(i, n)
	{
		forn(j, i)
		{
			L[i][j] = res[i][j];
		}
		
		L[i][i] = 1;
		
		fornx(j, i, n)
		{
			U[i][j] = res[i][j];
		}
	}
	
	Vector y (n);
	backwardSubs(L, b, y, n);	
	forwardSubs(U, y, x, n);	
	
	return;
}

void matrizInicial(Matriz& A, Matriz& hil, Vector& pos, Vector& x, int n)
{		
	/* Generar posición random como aproximación del enemigo. */	
	generarVector(pos, n);
	buscarMatriz(A, hil, pos, x, n);
	
	return;
}

void Hilbert(Matriz& A, int n)
/* Calcula una matríz de Hilbert de nxn elementos */
{
	forn(i, n)
	{
		forn(j, n)
		{
			A[i][j] = 1.0/(i+j+1);
			/* Porque en la definición empiezan a contar desde 1, entonces quedaría 
			 * (i+1) + (j+1) - 1 = i+j+1  */
		}
	}
}

/* matriz / matrix*x = aproximacion 
 * matriz[i][i] = aproximacion[i]/x[i] */
void buscarMatriz(Matriz& matriz, Matriz& hil, Vector& pos_enemigo, Vector& pos_propia, int n)
{
	/* diag*hilbert*x = y 
	 * diag*(hilbert*x) = y*/
	Vector xAux (n, 0);
	matrizPorVector(hil, pos_propia, xAux, n);
	 
	/* Llenar los elementos de la diagonal, se puede hacer con un vector. */
	Vector diag (n);	
	forn(i, n)	
		diag[i] = dividirEnRango(pos_enemigo[i], xAux[i]);
	
	diagonalPorMatriz(diag, hil, matriz, n);
	
	return;
}
